#include <iostream>
#include <fstream>

#include <GL/glx.h>
#include <GL/gl.h>

#include <SDL/SDL.h>
#include <GL/glut.h>

#include <cv.h>
#include <highgui.h>


using namespace cv;
using namespace std;

double unitMire = 1;/** < Unité de la mire. */ //0.027;
float w = 640; /** < Largeur de la vidéo. */
float h = 480; /** < hauteur de la vidéo. */
int largeur = 4; /** < Largeur de la mire. */
int hauteur = 3; /** < Hauteur de la mire. */
int frameTime = 200; /** < temps d'attente entre 2 frames pour l'affichage de la video. */

string nomVideoMire = "mire1_4x3x27mm.avi"; /** < Nom de la vidéo de mire à charger. */
string nomFichierCoordonnesInt = "intrinseques.txt"; /** < Nom fichier coords intrinseques. */

Mat A(3, 3, DataType<double>::type); /** <  cameraMatrix */
Mat K(1, 4, DataType<double>::type); /** < distCoeffs */
Mat R(3, 3, DataType<double>::type); /** < rvecs */
Mat T(1, 4, DataType<double>::type); /** <  tvecs */

GLuint texture; /** < texture issue de la video. */
cv::Mat frame,im; // object to receive a captured frame.
CvMat *_img = 0;
CvMat stub;

/** @brief Initialisation de la texture. */
void initTexture() {
  glGenTextures(1, &texture);

  glBindTexture( GL_TEXTURE_2D, texture ); //bind the texture to it's array
  glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
}

/** @brief Mise à jour de le texture à partir de la frame de la vidéo. */
void updateTexture()
{
    if (_img == 0)
        _img = new CvMat(frame);

    CvArr *arr = _img;
    CvMat *arrMat, *cvimage;
    arrMat = cvGetMat(arr, &stub);
    cvimage = cvCreateMat(arrMat->rows, arrMat->cols, CV_8UC3);
    cvConvertImage(arrMat, cvimage, 0);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, cvimage->cols, cvimage->rows,0, GL_BGR, GL_UNSIGNED_BYTE, cvimage->data.ptr);
}

/** @brief Procédure qui va dessiner le plan avec la texture issue de la vidéo plaquée dessus. */
void dessinePlan ()
{
  glBindTexture( GL_TEXTURE_2D, texture ); //bind the texture
  glBegin (GL_QUADS);
    float x = w/1000 ,y = h/1000;
    glTexCoord2d(1.0,0.0); glVertex3d(+x,-y,1.); //with our vertices we have to assign a texcoord
    glTexCoord2d(1.0,1.0); glVertex3d(+x,+y,1.); //so that our texture has some points to draw to
    glTexCoord2d(0.0,1.0); glVertex3d(-x,+y,1.);
    glTexCoord2d(0.0,0.0); glVertex3d(-x,-y,1.);
  glEnd();
}

/** @brief Dessine un cube. */
void dessineCube()
{
  glBegin (GL_QUADS);
  glColor3ub(255,0,0);
    glVertex3i(1,1,1);  glVertex3i(1,-1,1); glVertex3i(-1,-1,1); glVertex3i(-1,1,1);
    glVertex3i(1,1,-1); glVertex3i(1,-1,-1);glVertex3i(-1,-1,-1);glVertex3i(-1,1,-1);
    glVertex3i(1,1,1);  glVertex3i(1,-1,1); glVertex3i(1,-1,-1); glVertex3i(1,1,-1);
    glVertex3i(-1,1,1); glVertex3i(-1,-1,1);glVertex3i(-1,-1,-1);glVertex3i(-1,1,-1);
    glVertex3i(-1,1,-1);glVertex3i(-1,1,1); glVertex3i(1,1,1);   glVertex3i(1,1,-1);
    glVertex3i(-1,-1,-1);glVertex3i(-1,-1,1);glVertex3i(1,-1,1);glVertex3i(1,-1,-1);
    glColor3ub(255,255,255);
  glEnd();
}

void unproject(float *pi, float *pc)
{
//   pi est un point 2D du plan image (en pixels)
//  pc un point sur le rayon issu de pi
    float a = A.at<double>(0,0);
    float b = A.at<double>(1,1);
    float u = A.at<double>(0,2);
    float v = A.at<double>(1,2);

    pc[0] = (pi[0] - u) / a;
    pc[1] = (pi[1] - v) / b;
    pc[2] = pi[2];
}

/** @brief Retourne la distance entre 2 points (R³) */
float distance(float *p1, float *p2)
{
    return sqrt(pow(p2[0] - p1[0],2) + pow(p2[1] - p1[1],2)  + pow(p2[2] - p1[2],2));
}

/** @brief Calcule les valeurs du frustum pour définir la perspective. */
void calculerFrustum (const Mat& A,const Mat& K,float *frustum )
{
    float p1[3],p2[3],pU1[3],pU2[3];
    p1[0] = w/2; p1[1] = h/2; p1[2] = 1.;
    p2[0] = 0;   p2[1] = h/2; p2[2] = 1.;
    unproject(p1,pU1); unproject(p2,pU2);
    frustum[0] = -1.0f * distance(pU1,pU2); // left

    p2[0] = w;   p2[1] = h/2; p2[2] = 1.;
    unproject(p2,pU2);
    frustum[1] = 1.0f * distance(pU1,pU2); // right

    p2[0] = w/2;   p2[1] = 0; p2[2] = 1.;
    unproject(p2,pU2);
    frustum[2] = -1.0f * distance(pU1,pU2); // down

    p2[0] = w/2;   p2[1] = h; p2[2] = 1.;
    unproject(p2,pU2);
    frustum[3] = 1.0f * distance(pU1,pU2); // up

    frustum[4] = 1.0f; // near

    frustum[5] = 1000.0f; // far
}

/** @brief calcule la transformation GtoC, GToC = GlobalToCamera = MireToCamera. */
void calculerTransformation(const Mat& R,const Mat& T,float *GtoC)
{
//    r00 r10 r20 t0
//    r01 r11 r21 t1
//    r02 r12 r22 t2
//    0   0   0  1.0
    GtoC[0] = R.at<double>(0,0); // colonne 1
    GtoC[1] = R.at<double>(1,0);
    GtoC[2] = R.at<double>(2,0);
    GtoC[3] = 0.0;
    GtoC[4] = R.at<double>(0,1); // colonne 2
    GtoC[5] = R.at<double>(1,1);
    GtoC[6] = R.at<double>(2,1);
    GtoC[7] = 0.0;
    GtoC[8] = R.at<double>(0,2); // colonne 3
    GtoC[9] = R.at<double>(1,2);
    GtoC[10]= R.at<double>(2,2);
    GtoC[11]= 0.0;
    GtoC[12]= T.at<double>(0,0);//0.0; // colonne 4
    GtoC[13]= T.at<double>(0,1);//0.0;
    GtoC[14]= T.at<double>(0,2);//0.0;
    GtoC[15]= 1.0;
}

/** @brief dessine les axes X, Y, Z au point (0,0,0). */
void dessineAxes(int taille)
{
    glColor3f(1.0, 0.0, 0.0);
    glBegin(GL_LINES);
        glVertex3f(0, 0, 0); // x
        glVertex3f(taille, 0, 0);

    glColor3f(0.0, 1.0, 0.0);
        glVertex3f(0, 0, 0);// y
        glVertex3f(0, taille, 0);

    glColor3f(0.0, 0.0, 1.0);
        glVertex3f(0, 0, 0); // Z
        glVertex3f(0, 0, taille);
    glEnd();
}

/** @brief dessine la mire dans son repère. */
void dessineMire(const int largeur, const int hauteur, const double taille_carreau)
{
    double t = taille_carreau;
    glBegin(GL_QUADS);

        for(int i = 0; i <= hauteur; i++)
        {
            for(int j = 0; j <= largeur; j++)
            {
                if((j+i)%2)  glColor3f(1.0, 1.0, 1.0);
                else     glColor3f(0.0, 0.0, 0.0);

                glVertex3f(t*i, t*j, 0);
                glVertex3f(t*i + t, t*j, 0);
                glVertex3f(t*i + t, t*j + t, 0);
                glVertex3f(t*i, t*j + t, 0);
            }
        }

    glEnd();
}

void glDrawFromCamera(const Mat& A,const Mat& K,const Mat& R,const Mat& T )//A, K, R, T
{
    glClear(GL_COLOR_BUFFER_BIT);

    float frustum[6], GtoC[16];
        calculerFrustum(A,K,frustum);
        calculerTransformation(R,T,GtoC);

    glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        glFrustum( frustum[0], frustum[1], frustum[2], frustum[3], frustum[4], frustum[5]);
//    gluPerspective(50,  w/h,  0.1,  1000.0);

    glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();

    /// passe dans le repère caméra
        gluLookAt(0., 0., 0., 0, 0, 1, 0., -1.0, 0.);

    /// dessin du plan texturé
        updateTexture();
        glEnable( GL_TEXTURE_2D );
            dessinePlan();
        glDisable( GL_TEXTURE_2D );

    /// passe dans le repère global
        glMultMatrixf(GtoC);

        dessineAxes(50);
        dessineMire( largeur, hauteur, unitMire);
        glTranslatef(2,2,0);
        dessineCube();

    glFlush();
}

int main(int argc, char *argv[])
{
//    bool continuer = true;

    if(argc >= 3)
    {
        largeur= atoi(argv[1]);
        hauteur= atoi(argv[2]);

        if(argc >= 4)
            {
                nomVideoMire = argv[3];

                if(argc >= 5)
                {
                    nomFichierCoordonnesInt = argv[4];

                    if(argc >= 6)
                    {
                        frameTime = atoi(argv[5]);

                        if(argc > 6)
                        {
                            cout << "Trop d'arguments." << endl;
                            cout << "./TP3 largeurMire hauteurMire nomVideo nomFichierCoordIntrinseques tempsEntre2FramesMs" << endl;
                            return -1;
                        }
                    }

                }
            }
    }
    else
    {
        cout << "Chargement des parametres par defaut : " << largeur << " " << hauteur << " " << nomVideoMire <<" " <<
                                                        nomFichierCoordonnesInt << " "  << frameTime  << endl;
        cout << "./TP3 largeurMire hauteurMire nomVideo nomFichierCoordIntrinseques tempsEntre2FramesMs" << endl;
    }

    ifstream fCoordIntrin(nomFichierCoordonnesInt.c_str());

    /// récupération des coordonnées intrinseques
    if(fCoordIntrin)
    {
        cout << "Chargement des coordonnees intrinseques. " << endl;
        string mot;
        double val;

        fCoordIntrin >> mot;
        for(int i = 0; i < 4; i ++)
        {
            fCoordIntrin >> val;
            K.at<double>(0,i) = val;
        }

        fCoordIntrin >> mot;
        for(int r = 0; r < A.rows ; r++)
        {
            for(int c = 0; c < A.cols ; c++)
               {
                 fCoordIntrin >> val;
                 A.at<double>(r,c) = val;

               }
        }
    }
    else { cout << "Erreur chargement coordonnées intrinseques." << endl; return -1;}

    /// initialisation SDL
    SDL_Init(SDL_INIT_VIDEO);
    SDL_WM_SetCaption("TP3 : LANCE-POLI",NULL);
    SDL_SetVideoMode(w, h, 32, SDL_OPENGL);


    /// chargement de la vidéo mire
    cv::VideoCapture cap(nomVideoMire);
    if(!cap.isOpened())  // check if we succeeded
        return -1;

    cv::namedWindow("Pattern",CV_WINDOW_AUTOSIZE);

    /// initialisation de vec3f
    vector<Point3f> v3f;
    for(int i = 0; i < hauteur; i++)
        for(int j = 0; j < largeur; j++)
            v3f.push_back(Point3f(unitMire*i,unitMire*j,0));

    int numFrame = 0;
    vector<vector<Point2f> > imagePoints;
    vector<vector<Point3f> > objectPoints;  //positions des coins de la mire dans l'espace

    while(cap.grab())
        {
            numFrame++;
            cap.retrieve(frame);

            char key = ' ';
            key =  cv::waitKey(frameTime);

            Size patternsize(largeur,hauteur); //interior number of corners
            vector<Point2f> corners;           //this will be filled by the detected corners

            bool patternfound = findChessboardCorners(frame, patternsize, corners,
                    CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE);

            objectPoints.push_back(v3f);
            imagePoints.push_back(corners);

            if(patternfound)
                drawChessboardCorners(frame, patternsize, Mat(corners), patternfound);

            Mat rvecs;
            Mat imagePointsM(corners);
            Mat objectPointsM(v3f);

            if(patternfound || corners.size() >0) // verif
            {
                solvePnP(objectPointsM,imagePointsM,A,K,rvecs,T, false);
                Rodrigues(rvecs, R);
                cout << "Calculs extrinseques effectués frame : " << numFrame << endl;


                glClear(GL_COLOR_BUFFER_BIT);

                /// init texture
                initTexture();

                /// calcul de la scène
                glDrawFromCamera(A, K, R, T);
                glFlush();
                SDL_GL_SwapBuffers();
            }

            cv::imshow( "Pattern",frame);

            if(key == 'q')
                  break;
        }


//    SDL_Event event;
//
//
//    while (continuer)
//    {
//
//        SDL_WaitEvent(&event);
//        switch(event.type)
//        {
//            case SDL_QUIT:
//                continuer = false;
//        }
//    }

    glDeleteTextures( 1, &texture );
    SDL_Quit();

    return 0;
}

